# Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import rosgraph
import rospy
import threading
import time
import os


def init_node():
    return {
        "pub": [],  # 发布了哪些topic
        "sub": [],  # 订阅了哪些topic
        "srv": [],  # 提供哪些服务
    }


rospy.init_node("maestro")


# def spin_thread():
#     try:
#         rospy.spin()
#     except Exception as e:
#         print("Exception:" + str(e))


# threading.Thread(target=spin_thread).start()


class RuntimeManager:
    instance = None

    @staticmethod
    def gi():
        if RuntimeManager.instance is None:
            RuntimeManager.instance = RuntimeManager()
        return RuntimeManager.instance

    def __init__(self):
        self.master = rosgraph.Master("/maestro")
        self.nodes = {}
        self.subTopics = {}
        self.oldState = None
        self.topic_to_msg_type = {}
        pass

    def isStateChanged(self):
        newState = self.master.getSystemState()
        if self.oldState != newState:
            self.oldState = newState
            self.Expand()
            return True
        return False

    def Expand(self):
        self.nodes = {}
        ss = self.master.getSystemState()
        for t in ss[0]:  # publishers
            for n in t[1]:  # nodes
                if n not in self.nodes:
                    self.nodes[n] = init_node()
                self.nodes[n]["pub"].append(t[0])
        for t in ss[1]:  # subscribers
            for n in t[1]:  # nodes
                if n not in self.nodes:
                    self.nodes[n] = init_node()
                self.nodes[n]["sub"].append(t[0])
        for t in ss[2]:  # services
            for n in t[1]:  # nodes
                if n not in self.nodes:
                    self.nodes[n] = init_node()
                self.nodes[n]["srv"].append(t[0])

    def QueryTopicMsgType(self, topicName):
        if topicName in self.topic_to_msg_type:
            return self.topic_to_msg_type[topicName]
        for topic, msg in rospy.get_published_topics():
            if topic == topicName:
                self.topic_to_msg_type[topicName] = msg
                return msg
        ret = os.popen("rostopic type %s" % topicName).read().strip()
        if ret and len(ret) > 0 and " " not in ret:
            self.topic_to_msg_type[topicName] = ret
            return ret
        print("unknown topic msg type:", topicName)
        return "unknown..."

    def add_subscribe_topic(self, name, cb, ws):
        with msg_locker:
            if name in self.subTopics:
                return
            msg = self.QueryTopicMsgType(name)
            # print("subscribe topic:", name, msg)
        if msg:
            with msg_locker:
                nodeName, msgName = msg.split("/")
                self.subTopics[name] = {
                    "topicName": name,
                    "obj": None,
                    "callback": cb,
                    "msg": msg,
                    "ws": ws,
                    "data": [],
                    "enable": True,
                }
            vars_dict = {
                "topicName": name,
                "callback": topicCallback,
            }
            exec(
                """from %s.msg import %s
subobj = rospy.Subscriber(topicName, %s, callback, callback_args=topicName)"""
                % (nodeName, msgName, msgName),
                globals(),
                vars_dict,
            )
            with msg_locker:
                self.subTopics[name]["obj"] = vars_dict["subobj"]

    def remove_subscribe_topic(self, name):
        with msg_locker:
            if name in self.subTopics:
                self.subTopics[name]["obj"].unregister()
                del self.subTopics[name]

    def put_subscribe_topic_ret(self, name, msg):
        with msg_locker:
            if name in self.subTopics:
                if len(self.subTopics[name]["data"]) >= 3:
                    self.subTopics[name]["data"].pop(0)
                self.subTopics[name]["data"].append(msg)
                return True
            return False

    def get_subscribe_topic_ret(self):
        with msg_locker:
            ret = []
            for name in self.subTopics:
                if (
                    self.subTopics[name]["enable"]
                    and len(self.subTopics[name]["data"]) > 0
                ):
                    # self.subTopics[name]["enable"] = False
                    self.subTopics[name]["data2"] = self.subTopics[name]["data"].pop(0)
                    ret.append(self.subTopics[name])
            return ret

    def enable_subscribe_topic(self, name):
        with msg_locker:
            if name in self.subTopics:
                self.subTopics[name]["enable"] = True

    def SubscribeTopic(self, topicName, operate, cb, ws):
        if operate == 0:  # 取消订阅
            self.remove_subscribe_topic(topicName)
        elif operate == 1:  # 开始订阅
            self.add_subscribe_topic(topicName, cb, ws)
        elif operate == 2:  # 收到订阅
            self.enable_subscribe_topic(topicName)
            
    def SendBackMsg(self):
        ret = self.get_subscribe_topic_ret()
        if len(ret) > 0:
            for r in ret:
                if not r["callback"](r["topicName"], r["msg"], r["data2"], r["ws"]):
                    self.remove_subscribe_topic(r["topicName"])


msg_locker = threading.Lock()


def topicCallback(msg, topicName):
    RuntimeManager.gi().put_subscribe_topic_ret(topicName, msg)


# def sendBackMsg():
#     while True:
#         ret = RuntimeManager.gi().get_subscribe_topic_ret()
#         if len(ret) > 0:
#             for r in ret:
#                 if not r["callback"](r["topicName"], r["msg"], r["data2"], r["ws"]):
#                     RuntimeManager.gi().remove_subscribe_topic(r["topicName"])
#         else:
#             time.sleep(1 / 60)


# threading.Thread(target=sendBackMsg).start()


from envmanager import EnvManager
from packagetool import PackageTool


def subscribe_topic_callback(topic_name, msg_type, msg, ws):
    print("abc", topic_name, msg_type)
    pt = PackageTool()
    pt.put_string("subscribe_topic_ret")
    pt.put_string(topic_name)
    pt.put_string(msg_type)
    msgObj = EnvManager.gi().GetMsgObj(msg_type)
    t1 = time.time()
    msgObj.put_msg(pt, msg)
    print(time.time() - t1)


if __name__ == "__main__":
    RuntimeManager.gi().Expand()
    RuntimeManager.gi().SubscribeTopic(
        "/usb_cam/image_raw", 1, subscribe_topic_callback, None
    )
    input()
    rospy.signal_shutdown("asdf")
    # print(rm.nodes)
    # print(rospy.get_param_names())

"""getSystemStat()得到的数据
[
    [
        ["/rosout_agg", ["/rosout"]],
        ["/rosout", ["/image_view", "/usb_cam"]],
        ["/image_view/parameter_descriptions", ["/image_view"]],
        ["/image_view/parameter_updates", ["/image_view"]],
        ["/usb_cam/image_raw/compressedDepth", ["/usb_cam"]],
        ["/usb_cam/image_raw/compressedDepth/parameter_descriptions", ["/usb_cam"]],
        ["/usb_cam/image_raw/compressedDepth/parameter_updates", ["/usb_cam"]],
        ["/usb_cam/image_raw/compressed", ["/usb_cam"]],
        ["/usb_cam/image_raw/compressed/parameter_descriptions", ["/usb_cam"]],
        ["/usb_cam/image_raw/compressed/parameter_updates", ["/usb_cam"]],
        ["/usb_cam/image_raw", ["/usb_cam"]],
        ["/usb_cam/image_raw/theora", ["/usb_cam"]],
        ["/usb_cam/image_raw/theora/parameter_descriptions", ["/usb_cam"]],
        ["/usb_cam/image_raw/theora/parameter_updates", ["/usb_cam"]],
        ["/usb_cam/camera_info", ["/usb_cam"]],
    ],
    [["/rosout", ["/rosout"]]],
    [
        ["/rosout/get_loggers", ["/rosout"]],
        ["/rosout/set_logger_level", ["/rosout"]],
        ["/image_view/get_loggers", ["/image_view"]],
        ["/image_view/set_logger_level", ["/image_view"]],
        ["/image_view/load_nodelet", ["/image_view"]],
        ["/usb_cam/get_loggers", ["/usb_cam"]],
        ["/image_view/unload_nodelet", ["/image_view"]],
        ["/usb_cam/set_logger_level", ["/usb_cam"]],
        ["/image_view/list", ["/image_view"]],
        ["/image_view/set_parameters", ["/image_view"]],
        ["/usb_cam/image_raw/compressedDepth/set_parameters", ["/usb_cam"]],
        ["/usb_cam/image_raw/compressed/set_parameters", ["/usb_cam"]],
        ["/usb_cam/image_raw/theora/set_parameters", ["/usb_cam"]],
        ["/usb_cam/set_camera_info", ["/usb_cam"]],
        ["/usb_cam/start_capture", ["/usb_cam"]],
        ["/usb_cam/stop_capture", ["/usb_cam"]],
    ],
]
通过Expand转换为：
{
    "/rosout": {
        "pub": ["/rosout_agg"],
        "sub": ["/rosout"],
        "srv": ["/rosout/get_loggers", "/rosout/set_logger_level"],
    },
    "/image_view": {
        "pub": [
            "/rosout",
            "/image_view/parameter_descriptions",
            "/image_view/parameter_updates",
        ],
        "sub": [],
        "srv": [
            "/image_view/get_loggers",
            "/image_view/set_logger_level",
            "/image_view/load_nodelet",
            "/image_view/unload_nodelet",
            "/image_view/list",
            "/image_view/set_parameters",
        ],
    },
    "/usb_cam": {
        "pub": [
            "/rosout",
            "/usb_cam/image_raw/compressedDepth",
            "/usb_cam/image_raw/compressedDepth/parameter_descriptions",
            "/usb_cam/image_raw/compressedDepth/parameter_updates",
            "/usb_cam/image_raw/compressed",
            "/usb_cam/image_raw/compressed/parameter_descriptions",
            "/usb_cam/image_raw/compressed/parameter_updates",
            "/usb_cam/image_raw",
            "/usb_cam/image_raw/theora",
            "/usb_cam/image_raw/theora/parameter_descriptions",
            "/usb_cam/image_raw/theora/parameter_updates",
            "/usb_cam/camera_info",
        ],
        "sub": [],
        "srv": [
            "/usb_cam/get_loggers",
            "/usb_cam/set_logger_level",
            "/usb_cam/image_raw/compressedDepth/set_parameters",
            "/usb_cam/image_raw/compressed/set_parameters",
            "/usb_cam/image_raw/theora/set_parameters",
            "/usb_cam/set_camera_info",
            "/usb_cam/start_capture",
            "/usb_cam/stop_capture",
        ],
    },
    "/rostopic_184905_1742452369155": {
        "pub": ["/rosout"],
        "sub": [],
        "srv": [
            "/rostopic_184905_1742452369155/get_loggers",
            "/rostopic_184905_1742452369155/set_logger_level",
        ],
    },
}
"""
